#!/usr/bin/env python
#  -*-coding:utf8 -*-
 
import rospy
import numpy as np
from std_msgs.msg import String
from hellobot_msgs.msg import msg_pose

def CheckPose():
    #根据imu计算当前位姿
    pose = np.ones(6)
    pose[0] = 6
    pose[1] = 8
    pose[2] = 1
    return pose

class IMU():
    def __init__(self):
        self.pub = rospy.Publisher('Pose', msg_pose, queue_size=10)
        rate = rospy.Rate(10)

        pose = msg_pose()
        while not rospy.is_shutdown():
            temp = CheckPose()
            pose.x = temp[0]
            pose.y = temp[1]
            pose.z = temp[2]
            pose.alpha = temp[3]
            pose.beta = temp[4]
            pose.gamma = temp[5]

            #rospy.loginfo("the pose is: %3.4f %3.4f %3.4f %3.4f %3.4f %3.4f\n",pose.x,pose.y,pose.z,pose.alpha,pose.beta,pose.gamma)
            self.pub.publish(pose)
            rate.sleep()
    


if __name__ == '__main__':
    rospy.init_node('IMU')
    try:
        IMU()
    except rospy.ROSInterruptException:
        pass
    rospy.spin()
